#include <jsoncpp/json/json.h>
#include <time.h>
#include <fstream>
#include <sstream>
#include <iostream>
#include <deque>

#include <opencv2/opencv.hpp>
#include <tf2/transform_datatypes.h>
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

#include "ubt_mqtt/ubt_communication.h"
#include "ubt_mqtt/base64.h"
#include "ubt_mqtt/node.h"
#include "ubt_com/ubt_com_util.h"
#include "ubt_mqtt/config.h"

#include "ament_index_cpp/get_package_share_directory.hpp"
namespace ubt {

enum // ms
{
    e_heart_beat_rate = 1000,           // 心跳
    e_state_status_manager_rate = 100,  // 状态机状态
    e_real_speed_rate = 100,            // 实时速度信息
    e_map_task_info_rate = 1500,        // 上报地图 / 任务信息 (1500 ms)
    e_agv_version_rate = 10000,         // 上报agv版本信息 (10000 ms)
    e_mqtt_connect_status_rate = 1000,  // 检测 mqtt 连接状态
    e_battery_data_rate = 1000,         // 电池信息上报 (1000 ms)
    e_wifi_info_rate = 1000           // wifi信息上报 (1000 ms)
};

const static uint32_t g_publish_message_rate[E_PUBLISH_MANAGER_MAX] = {
    e_heart_beat_rate,
    e_state_status_manager_rate,
    e_real_speed_rate,
    e_map_task_info_rate,
    e_agv_version_rate,
    e_mqtt_connect_status_rate,
    e_battery_data_rate,
    e_wifi_info_rate
}; // ms

CUbtCommunicationNode::CUbtCommunicationNode()
 : _CpuRatioData(0.0),
  _TotalOdom(0.0),
  _TotalTime(0.0),
  _MappingStatus(false),
  _UslamMapFinished(true)
{
    _pMapStr  = std::make_shared<CMapManager>();

    //_pMapStr  = std::make_unique<CMapManager>(_pNodeHandle);
    _pMqttStr = std::make_unique<CMqttClient>();
    _pMessageStr = std::make_unique<CMessageManager>();
    _pTaskManager = std::make_unique<CTaskManager>(_pMapStr);

    if (false == ubt_yaml::getValue(ConfigDataInfo::Instance().getServerParamerFilePath(), "ui_node", _MqttParamer))
    {
        _MqttParamer.name = "admin";
        _MqttParamer.password = "password";
        _MqttParamer.address = "making.ubtrobot.com";
        _MqttParamer.port = 1883;
    }

    //获取包的绝对路径
    try
    {
        std::string _graphSlamRos = ament_index_cpp::get_package_share_directory("laser_scan_graph_slam");
        // std::string _gmappingRos = ament_index_cpp::get_package_share_directory("gmapping");
        std::string _pyDemoPath = ament_index_cpp::get_package_share_directory("ubt_mqtt");
        std::string _ubtTrajectoryTrack = ament_index_cpp::get_package_share_directory("ubt_trajectory_track");
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR(node->get_logger(),"Failed to get package paths.");
    }
    

    node->declare_parameter<std::string>("version"," ");
    _ConfigVersion = node->get_parameter("version").as_string();
    // node->declare_parameter<std::string>("/system/map_yaml_path","/home/ubt/ubt_agv_install/cfg/maps/current_map.yaml");
    _mapOriginYaml = node->get_parameter("/system/map_yaml_path").as_string();
    // node->declare_parameter<std::string>("/system/map_data_dir","/home/ubt/ubt_agv_install/cfg/maps/");
    _mapDataDir = node->get_parameter("/system/map_data_dir").as_string();
    node->declare_parameter<std::string>("/system/qrcode_poses_yaml","/home/ubt/ubt_agv_install/cfg/maps/qrcode_poses.yaml");
    _qrcode_poses_yaml = node->get_parameter("/system/qrcode_poses_yaml").as_string();
    // node->declare_parameter<std::string>("/system/map_localpoint","/home/ubt/ubt_agv_install/cfg/maps/localpoint.yaml");
    _mapLocalPointYAML = node->get_parameter("/system/map_localpoint").as_string();

    // node->declare_parameter<std::string>("/system/cfg","/home/ubt/ubt_agv_install/config");
    std::string _server_cfg_path = node->get_parameter("/system/cfg").as_string();

    _task_yaml = _server_cfg_path + g_package_name +"/task.yaml";
    _originImage = _mapDataDir + "test2000_map.png";

    RCLCPP_INFO_STREAM(node->get_logger(),"server paramer (name: " << _MqttParamer.name << ", password: " << _MqttParamer.password << ", address: "
                    << _MqttParamer.address << ", port: " << _MqttParamer.port << ")");
}

CUbtCommunicationNode::~CUbtCommunicationNode()
{
}

bool CUbtCommunicationNode::Initialize()
{
    MqttMessageCallbackAdd();
    _pMapStr->UpdateMapInfo();
    PublishTaskMessage();

    std::string init_loc_name;
    std::string map_image_name;
    
    if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_USLAM)
    {
        init_loc_name = "initialpose_uslam";
        map_image_name = "slam_map";
    }
    else
    {
        init_loc_name = "initialpose";
        map_image_name = "real_time_image";
    }
    node->declare_parameter<std::string>("/agv_type","u1000");
    _Agv_type = node->get_parameter("/agv_type").as_string();
    node->declare_parameter<std::string>("/robot_version","0.0.0");
    _RobotVersion.ros = node->get_parameter("/robot_version").as_string();
    node->declare_parameter<std::string>("/mb_version","0.0.0");
    _RobotVersion.motor = node->get_parameter("/mb_version").as_string();
    node->declare_parameter<std::string>("/agv_loc_algs","uslam");
    _agv_loc_algs = node->get_parameter("/agv_loc_algs").as_string();
    // node->declare_parameter<std::string>("/system/qrcode_poses_yaml","/home/ubt/ubt_agv_install/cfg/maps/qrcode_poses.yaml");
    // qrcode_poses_yaml_path = node->get_parameter("/system/qrcode_poses_yaml").as_string();
	_pubUbtUpdate     = node->create_publisher<std_msgs::msg::UInt8>("ubt_update", 1);
    _pubMapUpdate     = node->create_publisher<std_msgs::msg::String>("ui_map_update", 1);
    _pubMutiMapList   = node->create_publisher<std_msgs::msg::String>("muti_map_list", 1);
    _pubFixedPathTask = node->create_publisher<std_msgs::msg::String>("ui_fixedpath_task", 1);
    _pubRealTaskLimit = node->create_publisher<std_msgs::msg::String>("task_limit_index", 1);
    _pubCmdState      = node->create_publisher<std_msgs::msg::String>("ui_state", 1);
    _pubConfirm       = node->create_publisher<std_msgs::msg::String>("pose_confirm", 1);
    _pubInitLocation  = node->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(init_loc_name, 1);
    _pubCtrlCarto     = node->create_publisher<std_msgs::msg::String>("ui_stop_carto", 1);
    _pubCtrlGmapping  = node->create_publisher<std_msgs::msg::Int32>("task_gammping_control", 1);
    _pubMqttStatus    = node->create_publisher<std_msgs::msg::UInt8>("mqtt_status", 1);
    _pubAgvTaskId     = node->create_publisher<std_msgs::msg::String>("agv_task_id", 1);
    _pubAgvTaskName   = node->create_publisher<std_msgs::msg::String>("agv_task_name", 1);
    _pubCleanRecTask  = node->create_publisher<std_msgs::msg::UInt8>("clean_recover_task", 1);
    _pubCharacteristic= node->create_publisher<std_msgs::msg::UInt8>("ui_characteristic", 1); //特征扫描
    _pubChargeCtrl    = node->create_publisher<std_msgs::msg::UInt32>("charge_ctrl", 1); //打开充电回路
    _pubLiftSpeed     = node->create_publisher<std_msgs::msg::Float32>("lift_speed", 1); //顶升控制
    _pubEventReset    = node->create_publisher<std_msgs::msg::UInt32>("event_reset", 1); //事件复位
    _pubTwist         = node->create_publisher<geometry_msgs::msg::Twist>("ui_cmd_vel", 1); //速度控制
    _pubAgvBattery    = node->create_publisher<std_msgs::msg::String>("agv_battery", 1); //电池电量
    _pubLiftSpeedAndDistance = node->create_publisher<std_msgs::msg::Float32MultiArray>("lift_speed_and_distance", 1); //f1200s货叉控制
    _pubTractionLatchCtrl    = node->create_publisher<std_msgs::msg::UInt32>("traction_latch_ctrl", 1); //牵引销控制
    _pubLiftingJackCtrl      = node->create_publisher<std_msgs::msg::UInt32>("lifting_jack_ctrl", 1); //驱动轮控制
    _pubUslamMappingCtrl     = node->create_publisher<std_msgs::msg::Int64>("mappingstate", 1); //
    _pubInitFindRfid         = node->create_publisher<std_msgs::msg::String>("init_find_rfid", 1); //ui发送寻找rfid定位
    _pubUiConfigUpdate       = node->create_publisher<std_msgs::msg::String>("ui_config_update", 1); //通知其他模块ui参数已经更新
    _pubRemoteEmergency      = node->create_publisher<std_msgs::msg::UInt32>("remote_emergency", 1); //远程软急停   
    _pubLiftActionPause      = node->create_publisher<std_msgs::msg::UInt32>("lift_action_pause", 1); //转发倒退取货取消任务

    _subAgvStatus = node->create_subscription<std_msgs::msg::Int16MultiArray>("/agv/status", 1, std::bind(&CUbtCommunicationNode::AgvStatusCallback, this, std::placeholders::_1));
    _subAgvDiagWarn = node->create_subscription<std_msgs::msg::String>("/agv/diag_warn", 1, std::bind(&CUbtCommunicationNode::AgvDiagWarnCallback, this, std::placeholders::_1));
    _subCurrentPose = node->create_subscription<geometry_msgs::msg::Pose2D>("current_pose", 1, std::bind(&CUbtCommunicationNode::CurrentPoseCallback, this, std::placeholders::_1));
    _subStateStatus = node->create_subscription<std_msgs::msg::String>("ubt_state_status", 10, std::bind(&CUbtCommunicationNode::StateStatusCallback, this, std::placeholders::_1));
    _subCpuRatio = node->create_subscription<std_msgs::msg::Float32MultiArray>("cpu_ratio", 1, std::bind(&CUbtCommunicationNode::CpuRatioCallback, this, std::placeholders::_1));
    _subTaskStaus = node->create_subscription<std_msgs::msg::String>("task_status", 5, std::bind(&CUbtCommunicationNode::TaskStatusCallback, this, std::placeholders::_1));
    _subRealSpeed = node->create_subscription<geometry_msgs::msg::Twist>("real_speed", 5, std::bind(&CUbtCommunicationNode::RealSpeedCallback, this, std::placeholders::_1));
    _subResetDispath = node->create_subscription<std_msgs::msg::String>("reset_dispatch", 5, std::bind(&CUbtCommunicationNode::ResetDispatchCallback, this, std::placeholders::_1));
    _subRobotWorkInfo = node->create_subscription<std_msgs::msg::Float64MultiArray>("robot_work_info", 5, std::bind(&CUbtCommunicationNode::RobotWorkInfoCallback, this, std::placeholders::_1));
    _subMapConfidence = node->create_subscription<std_msgs::msg::Float32>("map_confidence", 5, std::bind(&CUbtCommunicationNode::MapConfidenceCallback, this, std::placeholders::_1));
    _subMapData = node->create_subscription<nav_msgs::msg::OccupancyGrid>(map_image_name, 5, std::bind(&CUbtCommunicationNode::MapImageCallback, this, std::placeholders::_1));
    _subSlamMapFinish = node->create_subscription<nav_msgs::msg::OccupancyGrid>("slam_map_finish", 5, std::bind(&CUbtCommunicationNode::SlamMapFinishImageCallback, this, std::placeholders::_1));
    _subRealRouteInfo = node->create_subscription<std_msgs::msg::Int16MultiArray>("real_route_info", 5, std::bind(&CUbtCommunicationNode::RealRouteInfoCallback, this, std::placeholders::_1));
    _subLaserPoints = node->create_subscription<sensor_msgs::msg::LaserScan>("/r2000_node/scan", 5, std::bind(&CUbtCommunicationNode::LidarLaserPointsCallback, this, std::placeholders::_1));
    _subEventLimit = node->create_subscription<std_msgs::msg::UInt32>("event_limit", 5, std::bind(&CUbtCommunicationNode::EventLimitCallback, this, std::placeholders::_1));
    _subAgvLastTask = node->create_subscription<std_msgs::msg::String>("agv_last_task", 5, std::bind(&CUbtCommunicationNode::OnAgvLastTaskCallback, this, std::placeholders::_1));
    _subBatteryData = node->create_subscription<ubt_interface::msg::BatteryData>("BatteryData", 5, std::bind(&CUbtCommunicationNode::OnBatteryDataCallback, this, std::placeholders::_1));
    _subWifiInfo = node->create_subscription<ubt_interface::msg::WifiInfo>("wifi_info", 5, std::bind(&CUbtCommunicationNode::OnWifiInfoCallback, this, std::placeholders::_1));

    // _reload_qrcode_position_client          = _pNodeHandle->serviceClient<ubt_odom::odomreset>("/odom_reset");

    std::thread mqtt_thread(&CUbtCommunicationNode::MqttThread, this);
    mqtt_thread.detach();

    std::thread loop_thread(&CUbtCommunicationNode::LoopThread, this);
    loop_thread.detach();

    return true;
}

void CUbtCommunicationNode::MqttThread(void)
{
    if (_pMqttStr)
    {
        //_pMqttStr->username_pw_set(_MqttParamer.name.c_str(), _MqttParamer.password.c_str());
        _pMqttStr->Connect(_MqttParamer.address, _MqttParamer.port);

        _pMqttStr->loop_forever();
    }

    mosqpp::lib_cleanup();
}

void CUbtCommunicationNode::AgvStatusCallback(const std_msgs::msg::Int16MultiArray::SharedPtr msgs)
{
    std::string data = _pMessageStr->PackAgvStatusMessage(msgs->data);
    _pMqttStr->Publish("agv/publicMsg/state/", data.length(), data.c_str());
}

void CUbtCommunicationNode::AgvDiagWarnCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    std::string data = _pMessageStr->PackAgvDiagWarnMessage(msgs->data);
    _pMqttStr->Publish("agv/publicMsg/warn", data.length(), data.c_str());
}

void CUbtCommunicationNode::CurrentPoseCallback(const geometry_msgs::msg::Pose2D::SharedPtr msgs)
{
    location_pose_t pose;
    location_pose_t destpose = {0.0, 0.0, 0.0};

    pose.x = msgs->x;
    pose.y = msgs->y;
    pose.theta = msgs->theta;

    _pMapStr->WorldToMap(pose, destpose);

    std::string data = _pMessageStr->PackLocalizationMessage(destpose, _pMapStr->getMapConfidence());
    _pMqttStr->Publish("agv/publicMsg/localization/", data.length(), data.c_str());
}

void CUbtCommunicationNode::OnBatteryDataCallback(const ubt_interface::msg::BatteryData::SharedPtr msg) {
    Json::Value data;
    data["perpower"] = ubt_utils::calcBatteryLevel(msg->perpower);

    std_msgs::msg::String pubAgvBattery;
    pubAgvBattery.data = std::to_string(data["perpower"].asInt());
    _pubAgvBattery->publish(pubAgvBattery);

    if (_PublishMamager[E_BATTERY_DATA_MESSAGE].need_send(g_publish_message_rate[E_BATTERY_DATA_MESSAGE]) == true)
    {
        std::string MqttData = _pMessageStr->PackBatteryDataMessage(data);
        _pMqttStr->Publish("agv/publicMsg/battery/", MqttData.length(), MqttData.c_str());
    }
}

void CUbtCommunicationNode::OnWifiInfoCallback(const ubt_interface::msg::WifiInfo::SharedPtr msg) {
    Json::Value data;
    data["net_ssid"] = msg->wifiname;
    data["ip"] = msg->wifiaddr;

    const char* cmd = "curl ifconfig.me";
    std::string output = exec(cmd);
    
    // Splitting the output by newline and getting the last line
    size_t pos = output.find_last_of('\n');
    std::string lastLine = output.substr(pos + 1);
    data["addr"] = lastLine;

    if (_PublishMamager[E_WIFI_INFO_MESSAGE].need_send(g_publish_message_rate[E_WIFI_INFO_MESSAGE]) == true)
    {
        std::string MqttData = _pMessageStr->PackWifiDataMessage(data);
        _pMqttStr->Publish("agv/publicMsg/network/", MqttData.length(), MqttData.c_str());
    }
}

std::string CUbtCommunicationNode::exec(const char* cmd) {
    char buffer[128];
    std::string result = "";
    std::shared_ptr<FILE> pipe(popen(cmd, "r"), pclose);
    if (!pipe) throw std::runtime_error("popen() failed!");
    while (!feof(pipe.get())) {
        if (fgets(buffer, 128, pipe.get()) != nullptr)
            result += buffer;
    }
    return result;
}

/*
msgs.data:
{
    "stamp": 123,
    "state":"UBT_WAIT",
    "hw_upcode": {
        "state":1,
        "tag":123,
        "pose": {
            "x":0.102,
            "y":0.023,
            "theta":1.24
        }
    },
    "hw_liftstate": 1
}
*/
void CUbtCommunicationNode::StateStatusCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    Json::Value root;

    if (ubt_utils::PraseJsonString(msgs->data, root) == false)
    {
        return;
    }

    _StateInfo.UpdateState(root["state"].asString());

    if (_PublishMamager[E_STATE_STATUS_MANAGER].need_send(g_publish_message_rate[E_STATE_STATUS_MANAGER]) == true)
    {
        std::string data = _pMessageStr->PackStateStatusMessage(root, _StateInfo.getStateDuration(), _CpuRatioData);
        _pMqttStr->Publish("agv/publicMsg/status/", data.length(), data.c_str());
    }
}

void CUbtCommunicationNode::CpuRatioCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msgs)
{
    _CpuRatioData = msgs->data[0];
}

/*
msgs.data:
{
    "type":"task_status",
    "data": {
        "task_status": {
            "now_task_id":"",
            "status":"ERROR",
            "desc": "stopReason=10&stopReasonDesc=NAV_POSEJUMP",
            "action_index":"",
            "action_type":""
        }
    }
}
*/
void CUbtCommunicationNode::TaskStatusCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    RCLCPP_INFO_STREAM(node->get_logger(),"TaskStatusCallback: " << msgs->data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(msgs->data, root) == false)
    {
        return;
    }

    std::string data = _pMessageStr->PackTaskStatusMessage(_pTaskManager->GetRequsetId(), root["data"]["task_status"]);
    _pMqttStr->Publish("agv/publicMsg/task_status/", data.length(), data.c_str());
}

void CUbtCommunicationNode::RealSpeedCallback(const geometry_msgs::msg::Twist::SharedPtr msgs)
{
    if (_PublishMamager[E_REAL_SPEED_MESSAGE].need_send(g_publish_message_rate[E_REAL_SPEED_MESSAGE]) == true)
    {
        std::string data = _pMessageStr->PackSpeedMessage(msgs->linear.x, msgs->angular.z, _TotalOdom, _TotalTime);
        _pMqttStr->Publish("agv/publicMsg/speed/", data.length(), data.c_str());
    }
}

void CUbtCommunicationNode::ResetDispatchCallback(const std_msgs::msg::String::SharedPtr msgs)
{
    std::string data = _pMessageStr->PackRestDispatchMessage(msgs->data);
    _pMqttStr->Publish("agv/publicMsg/reset_dispatch/", data.length(), data.c_str());
}

void CUbtCommunicationNode::RobotWorkInfoCallback(const std_msgs::msg::Float64MultiArray::SharedPtr msgs)
{
    _TotalOdom = msgs->data[1];
    _TotalTime = msgs->data[3];
}

void CUbtCommunicationNode::MapConfidenceCallback(const std_msgs::msg::Float32::SharedPtr msgs)
{
    if (_pMapStr)
    {
        _pMapStr->setMapConfidence(msgs->data);
    }
}

void CUbtCommunicationNode::SlamMapFinishImageCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msgs)
{
    if (!_UslamMapFinished)
    {
        _UslamMapFinished = true;
        MapImageCallback(msgs);

        std_msgs::msg::String msg;

        msg.data = "stop_carto";
        _pubCtrlCarto->publish(msg);
    }
}

void CUbtCommunicationNode::MapImageCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msgs)
{
    if (_StateInfo.getCurState() != "UBT_MAPPING")
    {
        return;
    }

    RCLCPP_INFO_STREAM(node->get_logger(),"-- map image callback --");
    int width  = msgs->info.width;
    int height = msgs->info.height;
    std::vector<uint8_t> map;

    for (int i = 0; i < height; i++)
    {
        for (int j = 0; j < width; j++)
        {
            int index = j + (height - i - 1) * width;
            if (msgs->data[index] >= 0 && msgs->data[index] <= 100)
            {
                map.push_back((100 - msgs->data[index]) * 255 / 100);
            }
            else
            {
                map.push_back(128);
            }
        }
    }

    cv::Mat imgFromVector = cv::Mat(height, width, CV_8UC1, map.data()).clone();
    std::vector<uchar> buf;
    // std::vector<int> param = vector<int>(2);
    // param[0] = cv::IMWRITE_PNG_COMPRESSION;
    // param[1] = 3;
    cv::imencode(".png", imgFromVector, buf);
    std::string basedata = encodeBase64(std::string().assign((char*)buf.data(), buf.size()));

    location_pose_t pose;
    pose.x = msgs->info.origin.position.x;
    pose.y = msgs->info.origin.position.y;
    pose.theta = 0;

    std::string data = _pMessageStr->PackMapImageMessage(basedata, height, width, pose);
    std::string topic = "agv/post/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/mapping";
    _pMqttStr->Publish(topic.c_str(), data.length(), data.c_str());
}

void CUbtCommunicationNode::RealRouteInfoCallback(const std_msgs::msg::Int16MultiArray::SharedPtr msgs)
{
    std::string data = _pMessageStr->PackDispatchInfoMessage(msgs->data);
    std::string topic = "agv/dispatch/dispatch_info" + _pMapStr->getMapId();
    _pMqttStr->Publish(topic.c_str(), data.length(), data.c_str());
}

void CUbtCommunicationNode::LidarLaserPointsCallback(const sensor_msgs::msg::LaserScan::SharedPtr msgs)
{
    if (_openLidar)
    {    
        rclcpp::Time ros_timeStamp = rclcpp::Clock().now();

        if (_laser_to_robot_trans.empty() || _laser_to_robot_yaw == 0.0) {
            // try {
                // tf::StampedTransform transform;
                // tf_listener.lookupTransform("/base_link", msgs->header.frame_id, rclcpp::Time(0), transform);
                // tf::Vector3 trans = transform.getOrigin();
                // tf::Quaternion rot = transform.getRotation();
                // double roll, pitch, yaw;
                // tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
                // _laser_to_robot_trans.push_back(trans.x());
                // _laser_to_robot_trans.push_back(trans.y());
                // _laser_to_robot_yaw = yaw;
                // RCLCPP_INFO(node->get_logger(),"laser_to_robot x %.3f y %.3f yaw %.3f", trans.x(), trans.y(), yaw);
            //           } catch (tf::TransformException ex) {
            //     RCLCPP_WARN(node->get_logger(),"Can not transform laser scan");
            //     return;
            // }
            // ros2<
                // 创建保存坐标变换信息的缓冲区
                std::unique_ptr<tf2_ros::Buffer> tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
                // 创建坐标变换的监听器
                std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
                tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

                geometry_msgs::msg::TransformStamped trans;
                // 监听当前时刻源坐标系到目标坐标系的坐标变换
                try {
                    trans = tf_buffer_->lookupTransform(
                                "/base_link", msgs->header.frame_id, tf2::TimePointZero);
                } catch (const tf2::TransformException & ex) {
                    // 如果坐标变换获取失败，进入异常报告
                    RCLCPP_INFO(
                        node->get_logger(), "Could not transform %s to %s: %s",
                        "/base_link", msgs->header.frame_id.c_str(), ex.what());
                    return;
                }

                // 四元数转换为欧拉角
                tf2::Quaternion q(
                    trans.transform.rotation.x,
                    trans.transform.rotation.y,
                    trans.transform.rotation.z,
                    trans.transform.rotation.w);
                tf2::Matrix3x3 m(q);
                double roll, pitch, yaw;
                m.getRPY(roll, pitch, yaw);

                _laser_to_robot_trans.push_back(trans.transform.translation.x);
                _laser_to_robot_trans.push_back(trans.transform.translation.y);
                _laser_to_robot_yaw = yaw;
                
                RCLCPP_INFO(node->get_logger(),"laser_to_robot x %.3f y %.3f yaw %.3f", trans.transform.translation.x, trans.transform.translation.y, yaw);

            // ros2>
                

      
        }

        std::vector<location_pose_t> laser_points;
        for (size_t i = 0; i < msgs->ranges.size(); ++i) {
            double laser_range = msgs->ranges[i];
            double theta = msgs->angle_min + msgs->angle_increment * i;
            double x = _laser_to_robot_trans[0] + laser_range * cos(theta + _laser_to_robot_yaw);
            double y = _laser_to_robot_trans[1] + laser_range * sin(theta + _laser_to_robot_yaw);
            location_pose_t p;
            p.x = x;
            p.y = y;
            laser_points.push_back(p);
        }

        std::string data = _pMessageStr->PackLidarPointsDataMessage(laser_points);
        std::string topic = "agv/post/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/radarInfo";
        _pMqttStr->Publish(topic.c_str(), data.length(), data.c_str());
        _openLidar = false;
    }
}

void CUbtCommunicationNode::EventLimitCallback(const std_msgs::msg::UInt32::SharedPtr msgs)
{
    _eventLimit = *msgs;
    std::string topic = "agv/post/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/event_limit";
    if (_openEventLimit)
    {
        Json::Value dataJson;
        dataJson["cmd"] = Json::Value(msgs->data);
        std::string data = _pMessageStr->PackEventMessageData(dataJson);
        _pMqttStr->Publish(topic.c_str(), data.length(), data.c_str());
        _openEventLimit = false;
    }
}

void CUbtCommunicationNode::MqttMessageCallbackAdd()
{
    if (_pMqttStr)
    {
        // 命令处理
        std::string manual_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/manual_move";
        _pMqttStr->message_callback_add(manual_topic, std::bind(&CUbtCommunicationNode::OnManualMoveMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // 命令处理
        std::string command_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/command";
        _pMqttStr->message_callback_add(command_topic, std::bind(&CUbtCommunicationNode::OnCommandMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // 扫地图
        std::string mapping_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/mapping";
        _pMqttStr->message_callback_add(mapping_topic, std::bind(&CUbtCommunicationNode::OnMappingMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // 心跳
        std::string heartbeat_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/heartbeat";
        _pMqttStr->message_callback_add(heartbeat_topic, std::bind(&CUbtCommunicationNode::OnHeartbeatMessageCallback, this, std::placeholders::_1, std::placeholders::_2));
    
        // 设置地图
        std::string mission_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/mission";
        _pMqttStr->message_callback_add(mission_topic, std::bind(&CUbtCommunicationNode::OnMissionMessageCallback, this, std::placeholders::_1, std::placeholders::_2));
    
        // task
        std::string task_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/task";
        _pMqttStr->message_callback_add(task_topic, std::bind(&CUbtCommunicationNode::OnTaskMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // real_task
        std::string real_task_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/real_task";
        _pMqttStr->message_callback_add(real_task_topic, std::bind(&CUbtCommunicationNode::OnRealTaskMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // real_limit_task
        std::string real_limit_task_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/real_task_limit_index";
        _pMqttStr->message_callback_add(real_limit_task_topic, std::bind(&CUbtCommunicationNode::OnRealLimitTaskMessageCallback, this, std::placeholders::_1, std::placeholders::_2));
    
        // set config
        std::string config_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/config";
        _pMqttStr->message_callback_add(config_topic, std::bind(&CUbtCommunicationNode::OnConfigMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // get config
        std::string request_config_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/request_config";
        _pMqttStr->message_callback_add(request_config_topic, std::bind(&CUbtCommunicationNode::OnRequsetConfigMessageCallback, this, std::placeholders::_1, std::placeholders::_2));

        // get config
        std::string init_loc_topic = "agv/linten/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/init_loc";
        _pMqttStr->message_callback_add(init_loc_topic, std::bind(&CUbtCommunicationNode::OnInitLocationMessageCallback, this, std::placeholders::_1, std::placeholders::_2));
    }
}

void CUbtCommunicationNode::OnManualMoveMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    bool ret = false;
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    if (_StateInfo.getCurState() == "UBT_WAIT" || _StateInfo.getCurState() == "UBT_WAITPOSE" || _StateInfo.getCurState() == "UBT_MAPPING")
    {
        double target_vx = 0.1 * std::stod(root["data"]["vspeed"].asString());
        double target_vw = 0.2 * std::stod(root["data"]["wspeed"].asString());
        // 数据
        geometry_msgs::msg::Twist twistPub;
        if (target_vx > 1.5) {
            target_vx = 1.5;
        }
        if (target_vw > 2) {
            target_vw = 2;
        }
        twistPub.linear.x = target_vx;
        twistPub.angular.z = target_vw;

        _pubTwist->publish(twistPub);
    }
}

void CUbtCommunicationNode::OnCommandMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    bool ret = false;
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    std_msgs::msg::String msg;
    msg.data = root["data"]["cmd"].asString();
    _pubCmdState->publish(msg);

    if (msg.data == "mission_cancel" && (_StateInfo.getCurState() == "UBT_RUN" || _StateInfo.getCurState() == "UBT_SHELFUP")) //急停，防撞条触发也会发mission_cancel，所以在运行时发出这个就是界面上点选的取消任务
    {
        std::string yaml_data = "task_id: \n""task_name: \n";

        try 
        {
            std::ofstream file_stream("/home/ubt/ubt_agv_install/cfg/maps/last_task.yaml");
            file_stream << yaml_data; //将yaml数据保存在本地
            file_stream.close();
        } 
        catch (const std::exception& e) 
        {
            RCLCPP_ERROR(node->get_logger(),"写入失败----------");
        }

        std_msgs::msg::String pubTaskMsg;
        pubTaskMsg.data = "";
        std_msgs::msg::UInt8 pubCleanMsg;
        pubCleanMsg.data = 1;
        _pubAgvTaskId->publish(pubTaskMsg);
        _pubAgvTaskName->publish(pubTaskMsg);
        _pubCleanRecTask->publish(pubCleanMsg);
    }

    if (msg.data == "mission_action_cancel" && msg.data == "mission_cancel")
    {
        real_task_queue.clear(); //清空任务列表
        if(_Agv_type == "f1200")
        {
            std_msgs::msg::UInt32 pubCleanMsg;
            pubCleanMsg.data = 1;
            _pubLiftActionPause->publish(pubCleanMsg);
        }
    }

    if (msg.data == "scan_characteristic_on")
    {
        std_msgs::msg::UInt8 pubCharacteristicMsg;
        pubCharacteristicMsg.data = 1;
        _pubCharacteristic->publish(pubCharacteristicMsg);
        // std_srvs::srv::SetBool srv;
        // srv.request.data = true;
        // try {
        //     // 等待服务可用，超时时间为 1.0 秒
        //     // ros::service::waitForService("/enable_learn_qrcode_position", 1000);
            
        //     // 调用服务
        //     if (enableLearnQrcodePositionClient.call(srv)) {
        //         // 服务调用成功
        //         RCLCPP_INFO(node->get_logger(),"Service call successful");
        //     } else {
        //         // 服务调用失败
        //         RCLCPP_ERROR(node->get_logger(),"Failed to call service");
        //     }
        // } catch(const std::exception& e) {
        //     // 服务异常
        //     RCLCPP_WARN(node->get_logger(),"Service call failed: %s", e.what());
        // }

        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
        request->data = true;
        while (!enableLearnQrcodePositionClient->wait_for_service(1s)) { 
            RCLCPP_INFO(node->get_logger(),"/enable_learn_qrcode_position service not available, waiting again...");
        }
        auto result = enableLearnQrcodePositionClient->async_send_request(request); 
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
        {
            // 服务调用成功
            RCLCPP_INFO(node->get_logger(),"Service call successful");
        }
        else
        {
            // 服务调用失败
            RCLCPP_ERROR(node->get_logger(),"Failed to call service");
        }

    }

    if (msg.data == "scan_characteristic_off")
    {
        std_msgs::msg::UInt8 pubCharacteristicMsg;
        pubCharacteristicMsg.data = 0;
        _pubCharacteristic->publish(pubCharacteristicMsg);
        // 调用enable_learn_qrcode_position服务
        // std_srvs::srv::SetBool srv;
        // srv.request.data = false;
        // try {
        //     // 等待服务可用，超时时间为 1.0 秒
        //     ros::service::waitForService("/enable_learn_qrcode_position", 1000);
            
        //     // 调用服务
        //     if (enableLearnQrcodePositionClient.call(srv)) {
        //         // 服务调用成功
        //         RCLCPP_INFO(node->get_logger(),"Service call successful");
        //     } else {
        //         // 服务调用失败
        //         RCLCPP_ERROR(node->get_logger(),"Failed to call service");
        //     }
        // } catch(const std::exception& e){
        //     // 服务异常
        //     RCLCPP_WARN(node->get_logger(),"Service call failed: %s", e.what());
        // }
        // 结束时上传二维码数据
        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
        request->data = true;
        while (!enableLearnQrcodePositionClient->wait_for_service(1s)) { 
            RCLCPP_INFO(node->get_logger(),"/enable_learn_qrcode_position service not available, waiting again...");
        }
        auto result = enableLearnQrcodePositionClient->async_send_request(request); 
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
        {
            // 服务调用成功
            RCLCPP_INFO(node->get_logger(),"Service call successful");
        }
        else
        {
            // 服务调用失败
            RCLCPP_ERROR(node->get_logger(),"Failed to call service");
        }
    }

    if (msg.data == "open_charging_circuit")
    {
        std_msgs::msg::UInt32 pubChargeMsg;
        pubChargeMsg.data = 1;
        _pubChargeCtrl->publish(pubChargeMsg);
    }
    if (msg.data == "close_charging_circuit")
    {
        std_msgs::msg::UInt32 pubChargeMsg;
        pubChargeMsg.data = 0;
        _pubChargeCtrl->publish(pubChargeMsg);
    }

    if (msg.data == "remote_braking")
    {
        std_msgs::msg::UInt32 pubEmergencyMsg;
        pubEmergencyMsg.data = 1;
        _pubRemoteEmergency->publish(pubEmergencyMsg);
    }

    if (msg.data == "mission_f1200_lift_up")
    {
        std_msgs::msg::Float32MultiArray liftTypeandHeight;
        liftTypeandHeight.data.push_back(0.08);
        liftTypeandHeight.data.push_back(90.0);
        while (true)
        {
            if (_eventLimit.data == 2)
            {
                break;
            }
            _pubLiftSpeedAndDistance->publish(liftTypeandHeight);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    if (msg.data == "mission_f1200_lift_down")
    {
        std_msgs::msg::Float32MultiArray liftTypeandHeight;
        liftTypeandHeight.data.push_back(-0.08);
        liftTypeandHeight.data.push_back(0.0);
        while (true)
        {
            if (_eventLimit.data == 1)
            {
                break;
            }
            _pubLiftSpeedAndDistance->publish(liftTypeandHeight);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }

    if (msg.data == "mission_action_rise")
    {
        std_msgs::msg::Float32 speed;
        speed.data = 0.5;
        while (true)
        {
            if (_eventLimit.data == 2)
            {
                break;
            }
            _pubLiftSpeed->publish(speed);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }
    if (msg.data == "mission_action_fall")
    {
        std_msgs::msg::Float32 speed;
        speed.data = -0.5;
        while (true)
        {
            if (_eventLimit.data == 1)
            {
                break;
            }
            _pubLiftSpeed->publish(speed);
            rclcpp::sleep_for(std::chrono::milliseconds(100));
        }
    }

    if (msg.data == "mission_action_drag_lift_up")
    {
        std_msgs::msg::UInt32 pubTractionMsg;
        pubTractionMsg.data = 1;
        _pubTractionLatchCtrl->publish(pubTractionMsg);
    }
    if (msg.data == "mission_action_drag_lift_down")
    {
        std_msgs::msg::UInt32 pubTractionMsg;
        pubTractionMsg.data = 0;
        _pubTractionLatchCtrl->publish(pubTractionMsg);
    }

    if (msg.data == "mission_action_reset")
    {
        std_msgs::msg::UInt32 pubResetMsg;
        pubResetMsg.data = 1;
        _pubEventReset->publish(pubResetMsg);
    }

    if (msg.data == "mission_action_lifting_jack_up")
    {
        std_msgs::msg::UInt32 pubJackMsg;
        pubJackMsg.data = 1;
        _pubLiftingJackCtrl->publish(pubJackMsg);
    }
    if (msg.data == "mission_action_lifting_jack_down")
    {
        std_msgs::msg::UInt32 pubJackMsg;
        pubJackMsg.data = 0;
        _pubLiftingJackCtrl->publish(pubJackMsg);
    }

    if (msg.data == "request_radarInfo")
    {
        _openLidar = true;
    }
    if (msg.data == "request_traction_state")
    {
        _openTractionLatchState = true;
    } 
    if (msg.data == "request_event_limit")
    {
        _openEventLimit = true;
    }
    // if (msg.data == "upilot_update")
    // {
    //     // 设置一下路径参数
    //     std::string m_cmd = _pyDemoPath + "/../../../script/online_update.sh https://" + _mServiceIp + root["data"]["cmd"].asString();
    //     std_msgs::msg::UInt8 pubUpdateMsg;
    //     pubUpdateMsg.data = 1;
    //     _pubUbtUpdate->publish(pubUpdateMsg);
    //     std::system(m_cmd.c_str());
    // }
    
    _pubCmdState->publish(msg);

    SubscribeResponce(root["request_id"].asString(), true, "command message success!");
}

void CUbtCommunicationNode::OnMappingMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    bool cmd_handle_status = false;
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    //request_id
    std::string request_id = root["request_id"].asString();
    _mapRequestId = request_id;
    if (root["data"].isMember("res_topic")) 
    {
        _pubMappingSuccessTopic = root["data"]["res_topic"].asString();
    }

    std::string cmd = root["data"]["cmd"].asString();
    bool success = true;
    std::string message = _agv_loc_algs + " " + cmd + " mapping message execute failed!";
    if (cmd == "start_mapping")
    {
        if (_StateInfo.getCurState() == "UBT_WAIT" ||
            _StateInfo.getCurState() == "UBT_WAITPOSE" ||

            _StateInfo.getCurState() == "UBT_MAPPING" ||
            _StateInfo.getCurState() == "UBT_RMCTL")
        {
            cmd_handle_status = StartMapping();
        }
        else
        {
            message = "current ubt_state is: " + _StateInfo.getCurState() + " ,except UBT_WAIT or UBT_MAPPING!";
            success = false;
        }
    }
    else if (cmd == "stop_mapping")
    {
        if (_mappingType != nullptr) 
        {
            std::string mappingTypeStr(_mappingType);
            if (mappingTypeStr == "uslam" && _pubMappingSuccessTopic != "")
            {
                std_msgs::msg::Int64 uslamMsg;
                uslamMsg.data = 2;
                _pubUslamMappingCtrl->publish(uslamMsg);
                return;
            }
        }
        cmd_handle_status = StopMapping();
        pubMappingSuccessToWeb(_pubMappingSuccessTopic);
    }
    else if (cmd == "save_map")
    {
        cmd_handle_status = StopMapping();
    }
    else
    {
        success = false;
        message = "unknow cmd: " + cmd;

        RCLCPP_ERROR_STREAM(node->get_logger(),"ubknow cmd: " << cmd);
    }

    if (cmd_handle_status)
    {
        SubscribeResponce(root["request_id"].asString(), true, cmd + " mapping message success");
    }
    else
    {
        SubscribeResponce(root["request_id"].asString(), false, cmd + " mapping message fail");
        message = _agv_loc_algs + " " + cmd + " mapping message execute failed!";
        success = false;
    }

    RCLCPP_INFO(node->get_logger(),"[Ui_listener] on_message_maping_callback end");
}

void CUbtCommunicationNode::pubMappingSuccessToWeb(const std::string& topic) 
{
    if (topic.empty()) {
        RCLCPP_WARN(node->get_logger(),"[Ui_listener] pub_mapping_success_to_web: topic is empty");
        return;
    }

    Json::Value msg;
    msg["data"] = "ok";

    std::string payload = Json::FastWriter().write(msg);

    if (_pMqttStr->Publish(topic.c_str(), payload.length(), payload.c_str(), 0, false)) 
    {
        RCLCPP_INFO(node->get_logger(),"[Ui_listener] pub_mapping_success_to_web ok");
    } 
    else 
    {
        RCLCPP_ERROR(node->get_logger(),"[Ui_listener] Failed to publish mapping success message to web");
    }
}

void CUbtCommunicationNode::OnHeartbeatMessageCallback(const char* data, int len)
{
    if (_pMessageStr)
    {
        _pMessageStr->PraseHeartBeatMessage(data, len);
    }
}

void CUbtCommunicationNode::OnMissionMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__);
    Json::Value root;
    int has_new_map;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    bool success = true;
    std::string message = "mission message execute succeed!";

    if (_StateInfo.getCurState() != "UBT_WAIT" &&
        _StateInfo.getCurState() != "UBT_WAITPOSE" &&
        _StateInfo.getCurState() != "UBT_CHANGE" &&
        _StateInfo.getCurState() != "UBT_LIFTUP" &&
        _StateInfo.getCurState() != "UBT_DRAGUP")
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),__func__ << ", current state is: " << _StateInfo.getCurState());
        SubscribeResponce(root["request_id"].asString(), false, _StateInfo.getCurState() + "is error");
        return;
    }

    if (!_pMapStr->HandleMapInfoData(root["data"]["map"], [this]() {
        // 有新地图，通知算法更新
        std_msgs::msg::String msg;
        msg.data = "map_update_start";
        _pubMapUpdate->publish(msg);
    }))
    {
        SubscribeResponce(root["request_id"].asString(), false, "handle mission message fail");
        return;
    }

    std::pair<bool, std::string> result = PraseMissionMessage(root);
    success = result.first;
    message = result.second;    

    SubscribeResponce(root["request_id"].asString(), success, message);
}

void CUbtCommunicationNode::readMapParam(const std::string& orginImage, const std::string& mapOrginYaml) {
    std::pair<float, float> mapInfo = ubt_utils::getMapInfo(orginImage);
    std::vector<std::string> origin = ubt_utils::getMapOriginInfo(mapOrginYaml, "origin");

    _mapWidth = mapInfo.first;
    _mapHeight = mapInfo.second;
    if (!origin.empty()) {
        _map_x = std::stof(origin[0]);
        _map_y = std::stof(origin[1]);
    }
    RCLCPP_ERROR_STREAM(node->get_logger(), "Map width: " << _mapWidth << ", height: " << _mapHeight << ", map_x: " << _map_x << ", map_y: " << _map_y);
}

std::pair<bool, std::string> CUbtCommunicationNode::PraseMissionMessage(Json::Value root)
{
    bool success = true;
    std::string message = "mission message execute succeed!";
    std::vector<std::string> map_hash_last = ubt_utils::getMapOriginInfo(_mapOriginYaml, "map_hash");
    // std::string map_hash_last = ubt_utils::getMapOriginInfo(_mapOriginYaml, "map_hash");
    // map_id_split = root["data"]["map"]["map_id"].split("_");
    bool ret = ubt_utils::updateOriginYamlFile(root["data"]["map"], _originImage, _mapOriginYaml);

    if (!ret) {
        return {false, "read mission yaml is fail!"};
    } else {
        return {true, ""};
    }

    std::string map_type = root["data"]["map"]["map_type"].asString();
    if (map_type != "QRCODE") {
        std::string map_hash_cur = root["data"]["map"]["map_hash"].asString();
        if (map_hash_cur != map_hash_last[0]) {
            ret = false;

            if (map_type == "SLAM" && root["data"]["map"].isMember("map_url")) 
            {
                std::string map_dir = root["data"]["map"]["map_url"].asString();
                std::string map_url = "https://" + _MqttParamer.address + map_dir;

                httplib::Client cli(map_url.c_str());
                auto response = cli.Get("/");

                if (response && response->status == 200) {
                    ret = ubt_utils::directlySaveMapImage(response->body, _originImage);
                } else {
                    std::cerr << "Unable to obtain map from " << map_url << std::endl;
                }
            } else if (map_type == "3DSLAM" && root["data"]["map"]["pcd_map"].isMember("file") &&
                       !root["data"]["map"]["pcd_map"]["file"].empty()) {
                std::string map_dir = root["data"]["map"]["pcd_map"]["file"].asString();
                std::string map_url = "https://" + _MqttParamer.address + map_dir;

                httplib::Client cli(map_url.c_str());
                auto response = cli.Get("/");

                if (response && response->status == 200) {
                    ret = ubt_utils::saveMapImage3D(response->body, _mapDataDir);
                } else {
                    std::cerr << "Unable to obtain 3D map from " << map_url << std::endl;
                }
            } else if (root["data"]["map"].isMember("map")) {
                ret = ubt_utils::saveMapImage(root["data"]["map"]["map"].asString(), _originImage);
            }

            if (!ret) {
                return {false, "save map image is fail!"};
            }

            std::cout << "map_update_start" << std::endl;
            std::cerr << "update_map--------------" << std::endl;
            Json::Value data;
            std::string time_str = ubt_utils::getTimeStr();
            std::string request_id = "send_" + time_str;
            data["map_id"] = root["data"]["map"]["map_id"];
            std::string topic = "agv/publicMsg/mission_reply";

            auto request = std::make_shared<std_srvs::srv::Trigger::Request>();
            while (!reloadMapClient->wait_for_service(1s)) { 
                RCLCPP_INFO(node->get_logger(),"/aeb/enable service not available, waiting again...");
            }
            auto result = reloadMapClient->async_send_request(request); 
            if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
            {
                data["success"] = 1;
                std::string dataPub = _pMessageStr->PackUpdateMapMessage(request_id, data);
                _pMqttStr->Publish(topic.c_str(), dataPub.length(), dataPub.c_str());
                RCLCPP_INFO(node->get_logger(),"Reload map service called successfully");
        }
            else
            {
                data["success"] = 0;
                std::string dataPub = _pMessageStr->PackUpdateMapMessage(request_id, data); 
                _pMqttStr->Publish(topic.c_str(), dataPub.length(), dataPub.c_str());
                RCLCPP_ERROR(node->get_logger(),"Reload map service failed with message: %s", result.get()->message.c_str());
                return {false, "reload map failed"};
            }

            // std_srvs::srv::Trigger srv;
            // Call the service
            // if (reloadMapClient.call(srv)) {
            //     if (srv.response.success) {
            //         data["success"] = 1;
            //         std::string dataPub = _pMessageStr->PackUpdateMapMessage(request_id, data);
            //         _pMqttStr->Publish(topic.c_str(), dataPub.length(), dataPub.c_str());
            //         RCLCPP_INFO(node->get_logger(),"Reload map service called successfully");
            //     } else {
            //         data["success"] = 0;
            //         std::string dataPub = _pMessageStr->PackUpdateMapMessage(request_id, data); 
            //         _pMqttStr->Publish(topic.c_str(), dataPub.length(), dataPub.c_str());
            //         RCLCPP_ERROR(node->get_logger(),"Reload map service failed with message: %s", srv.response.message.c_str());
            //         return {false, "reload map failed"};
            //     }
            // } else {
            //     RCLCPP_ERROR(node->get_logger(),"Failed to call reload map service");
            // }

            // 更新地图
            // if (os.environ.get('SLAM_TYPE') == "graph_slam") {
            std::string cmd = "bash " + _graphSlamRos + "/../../../script/graph_mapping.sh update";
            system(cmd.c_str());
        }
        
        readMapParam(_originImage, _mapOriginYaml);

        updateLocalPointFile(root['data']['map']['map_points']);

        _muti_map_list = root['data']['muti_map_list'][0]['map_points'];
        location_pose_t pose;
        location_pose_t destpose;
        pose.x = _muti_map_list["x"].asFloat();
        pose.y = _muti_map_list["y"].asFloat();
        pose.theta = _muti_map_list["t"].asFloat();
        _pMapStr->WorldToMap(pose, destpose);
        _muti_map_list["x"] = std::to_string(destpose.x);
        _muti_map_list["y"] = std::to_string(destpose.y);

        Json::StreamWriterBuilder writer;
        std::string jsonString = Json::writeString(writer, _muti_map_list);

        // 发布消息
        std_msgs::msg::String msg;
        msg.data = jsonString;
        _pubMutiMapList->publish(msg);

        std::ofstream mutiMapListFile(_muti_map_list_json);
        if (mutiMapListFile.is_open())
        {
            mutiMapListFile << jsonString;
        }

        // 读取上一个任务的 JSON 文件
        Json::Value fixedpath_task;
        std::ifstream lastTaskFile(_last_task_json);
        if (lastTaskFile.is_open()) {
            lastTaskFile >> fixedpath_task;
        }

        Json::Value qrcode_poses_data = get_qrcode_poses_by_map_list();
        std::ofstream QrcodePosesFile(_qrcode_poses_yaml);
        if (QrcodePosesFile.is_open())
        {
            Json::StreamWriterBuilder writerBuilder;
            std::string QrJsonString = Json::writeString(writerBuilder, qrcode_poses_data);
            QrcodePosesFile << QrJsonString;
        }
        auto request = std::make_shared<std_srvs::srv::SetBool::Request>();
        request->data = true;
        while (!reloadQrcodeClient->wait_for_service(1s)) { 
            RCLCPP_INFO(node->get_logger(),"/reload_qrcode_position service not available, waiting again...");
        }

        auto result = reloadQrcodeClient->async_send_request(request); 
        if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
        {

            RCLCPP_INFO(node->get_logger(),"Reload qrcode position succeeded");
        }
        else
        {

            RCLCPP_ERROR(node->get_logger(),"Reload qrcode position failed");
            // return;
        }


        // std_srvs::srv::SetBool srv;
        // srv.request.data = true;
        // if (reloadQrcodeClient.waitForExistence(ros::Duration(1.0))) {
        //     if (reloadQrcodeClient.call(srv)) {
        //         if (srv.response.success) {
        //             RCLCPP_INFO(node->get_logger(),"Reload qrcode position succeeded");
        //         } else {
        //             RCLCPP_ERROR(node->get_logger(),"Reload qrcode position failed");
        //         }
        //     } else {
        //         RCLCPP_ERROR(node->get_logger(),"Failed to call reload_qrcode_position service");
        //     }
        // } else {
        //     RCLCPP_ERROR(node->get_logger(),"Reload qrcode position service not available");
        // }
    }

    PublishTaskMessage();
    return std::make_pair(success, message);
}


Json::Value CUbtCommunicationNode::get_qrcode_poses_by_map_list() {
    Json::Value qrcode_poses_data;

    for (const auto& value : _muti_map_list) {
        if (!value["rfid_no"].asString().empty() && (value["type"].asString() == "qrcode-station-point" || value["type"].asString() == "shelves-point")) {
            Json::Value qrcode_pose;
            qrcode_pose["x"] = std::stof(value["x"].asString());
            qrcode_pose["y"] = std::stof(value["y"].asString());
            qrcode_pose["yaw"] = value["t"].asString();
            
            qrcode_poses_data[value["rfid_no"].asString()] = qrcode_pose;
        }
    }

    return qrcode_poses_data;
}

void CUbtCommunicationNode::OnAgvLastTaskCallback(const std_msgs::msg::String::SharedPtr msgs){
    // 打开 last_task.json 文件进行读取
    std::ifstream lastTaskFile(_last_task_json);
    Json::Value fixedpathTask;
    lastTaskFile >> fixedpathTask;

    // 将 JSON 数据发送到 fixedpath_task_pub
    // 请根据实际情况调整消息的字段和发布方式
    Json::StreamWriterBuilder writer;
    std::string jsonString = Json::writeString(writer, fixedpathTask);
    std_msgs::msg::String msgPub;
    msgPub.data = jsonString;
    _pubFixedPathTask->publish(msgPub);
}


void CUbtCommunicationNode::updateLocalPointFile(const Json::Value& mapPoints) {
    bool localPointUpdate = false;
    std::stringstream localPointYAML;
    localPointYAML << "system:" << std::endl << "  init_poses: ";

    for (const auto& mapPoint : mapPoints) {
        if (mapPoint["type"] == "charge-point" || mapPoint["type"] == "parking-point") {
            // std::cout << "mapPoint: " << mapPoint << std::endl;
            location_pose_t pose;
            location_pose_t destpose;
            pose.x = mapPoint["x"].asFloat();
            pose.y = mapPoint["y"].asFloat();
            pose.theta = mapPoint["t"].asFloat();
            _pMapStr->WorldToMap(pose, destpose);

            localPointYAML << "name=" << mapPoint["id"].asString() << "&type=" << mapPoint["type"].asString()
                            << "&x=" << pose.x << "&y=" << pose.y << "&theta=" << mapPoint["t"].asFloat() << ';';
            localPointUpdate = true;
        }
    }

    // 如果 mapPoints 没有 'charge-point' 或 'parking-point'，恢复还原成默认设置
    if (!localPointUpdate) {
        localPointYAML << "name=0&type=charge-point&x=0&y=0&theta=0;";
        localPointUpdate = true;
    }

    std::ofstream file;
    try {
        if (localPointUpdate) {
            // std::cout << "localPointYAML: " << localPointYAML.str() << std::endl;
            file.open(_mapLocalPointYAML, std::ios::out);
            file << localPointYAML.str();
            if (file.is_open()) {
                file.close();
            }
        }
    } catch (const std::exception& e) {
        std::cerr << "updateLocalPointFile error: " << e.what() << std::endl;
    } 
}

void CUbtCommunicationNode::OnTaskMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    if (_StateInfo.getCurState() == "UBT_WAIT" ||
        _StateInfo.getCurState() == "UBT_WAITPOSE" ||
        _StateInfo.getCurState() == "UBT_CHARGE" ||
        _StateInfo.getCurState() == "UBT_LIFTUP" ||
        _StateInfo.getCurState() == "UBT_DRAGUP" ||
        _StateInfo.getCurState() == "UBT_BREAKOPENED" ||
        _StateInfo.getCurState() == "UBT_STOP")
    {
        _pTaskManager->UpdateTaskInfo(root["data"]);
    }

    PublishTaskMessage();//read_and_send_agv_task
    SubscribeResponce(root["request_id"].asString(), true, "task message successed!");//topic_request
    if (root["data"].isMember("desp")) {
        if (root["data"]["desp"].asString() == "task completed") {
            std::string yamlData = "task_id: \n"
                                   "task_name: \n";
            try {
                std::ofstream file(_last_task_yaml);
                file << yamlData;
                file.close();
            } catch (const std::exception& e) {
                std::cerr << "写入失败: " << e.what() << std::endl;
            }

            // 假设以下为 ROS 发布消息的相应处理
            std_msgs::msg::String pubTaskMsg;
            pubTaskMsg.data = "";
            std_msgs::msg::UInt8 pubCleanMsg;
            pubCleanMsg.data = 1;
            _pubAgvTaskId->publish(pubTaskMsg);
            _pubAgvTaskName->publish(pubTaskMsg);
            _pubCleanRecTask->publish(pubCleanMsg);
        }
    }
}

void CUbtCommunicationNode::OnRealTaskMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    if (_StateInfo.getCurState() != "UBT_WAIT" &&
        _StateInfo.getCurState() != "UBT_WAITPOSE" &&
        _StateInfo.getCurState() != "UBT_DRAGUP" &&
        _StateInfo.getCurState() != "UBT_SHELFUP" &&
        _StateInfo.getCurState() != "UBT_LIFTUP")
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),__func__ << ", current state is: " << _StateInfo.getCurState());
        SubscribeResponce(root["request_id"].asString(), false, _StateInfo.getCurState() + "is error");
        _realTaskDeque.push_back(root);
        return;
    }

    _pTaskManager->HandleRealTaskData(root, [this](const std::string &data) {
        std_msgs::msg::String msg;

        msg.data = data;
        _pubFixedPathTask->publish(msg);
    });
    
    SubscribeResponce(root["request_id"].asString(), true, "real_task message successed!");
}

void CUbtCommunicationNode::OnRealLimitTaskMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;
    std_msgs::msg::String msg;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    msg.data = data;
    _pubRealTaskLimit->publish(msg);

    SubscribeResponce(root["request_id"].asString(), true, "real_task_limit message successed!");
}

void CUbtCommunicationNode::OnConfigMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    _ConfigVersion = root["data"]["version"].asString(); // 获取配置文件版本号

    try
    {
        Json::Value data = root["data"];
        Json::Value::Members mem = data.getMemberNames();
        for (auto iter = mem.begin(); iter != mem.end(); iter++)  
        {
            node->set_parameter(rclcpp::Parameter(*iter, data[*iter].asString())); // 将配置文件加载到参数服务器
        }

        std_msgs::msg::String pubConfigMsg;
        pubConfigMsg.data = "config";
        _pubUiConfigUpdate->publish(pubConfigMsg);

        UpdateConfigFile(data);
    }
    catch(const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"handle config message error: " << e.what() << '\n');
    }

    // SubscribeResponce(root["request_id"].asString(), true, "config message successed!");
}

void CUbtCommunicationNode::OnRequsetConfigMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    RequestConfigMessage(root);
    SubscribeResponce(root["request_id"].asString(), true, "request config message successed!");
}

void CUbtCommunicationNode::OnInitLocationMessageCallback(const char* data, int len)
{
    RCLCPP_INFO_STREAM(node->get_logger(),__func__ << ", msg: " << data);
    Json::Value root;

    if (ubt_utils::PraseJsonString(std::string((char*)data), root) == false)
    {
        return;
    }

    if (_StateInfo.getCurState() != "UBT_RUN" &&
        _StateInfo.getCurState() != "UBT_ACTION" &&
        _StateInfo.getCurState() != "UBT_PAUSE" &&
        _StateInfo.getCurState() != "UBT_OBSTACLE")
    {
        int comfirm = root["data"]["confirm"].asInt();
        if (comfirm == 0)
        {
            location_pose_t pose;
            location_pose_t destPose;

            pose.x = root["data"]["point"]["x"].asFloat();
            pose.y = root["data"]["point"]["y"].asFloat();
            pose.theta = root["data"]["point"]["t"].asFloat();

            _pMapStr->MapToWorld(pose, destPose);

            geometry_msgs::msg::PoseWithCovarianceStamped msg;
            msg.header.stamp = rclcpp::Clock().now();
            msg.header.frame_id = "map";
            msg.pose.pose.position.x = destPose.x;
            msg.pose.pose.position.y = destPose.y;
            msg.pose.pose.position.z = 0.0;
            geometry_msgs::msg::Quaternion qua = ubt_com::convertThetaToQuaternion(destPose.theta);
            msg.pose.pose.orientation = qua;
            msg.pose.covariance[6 * 0 + 0] = 0.5 * 0.5;
            msg.pose.covariance[6 * 1 + 1] = 0.5 * 0.5;
            msg.pose.covariance[6 * 3 + 3] = 3.1415 / 12.0 * 3.1415 / 12.0;

            _pubInitLocation->publish(msg);
        }
        else if (comfirm == 1)
        {
            std_msgs::msg::String msg;

            msg.data = "comfirm";
            _pubConfirm->publish(msg);
        }
        else if (comfirm == 2)
        {
            std_msgs::msg::String msg;

            msg.data = "1";
            _pubInitFindRfid->publish(msg);
        }

        SubscribeResponce(root["request_id"].asString(), true, "init loc message successed!");
    }
    else
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),__func__ << ", current state is: " << _StateInfo.getCurState());
        SubscribeResponce(root["request_id"].asString(), false, _StateInfo.getCurState() + " is error");
    }
}

void CUbtCommunicationNode::SubscribeResponce(std::string requsetid, bool status, std::string errmsg)
{
    std::string topic = "agv/post/" + ConfigDataInfo::Instance().getRobotSerialNumber() + "/message";
    std::string data = _pMessageStr->PackResponseMessage(requsetid.replace(0, 5, "res_"), status, errmsg); // strlen("send_") = 5

    _pMqttStr->Publish(topic.c_str(), data.length(), data.c_str());
}

void CUbtCommunicationNode::PublishTaskMessage()
{
    _TaskPublishData = _pMessageStr->PackTaskMessage(_pMapStr->getMapOriginInfo(), _pTaskManager->GetTaskInfo(), _ConfigVersion);
    _pMqttStr->Publish("agv/publicMsg/task/", _TaskPublishData.length(), _TaskPublishData.c_str());
}

void CUbtCommunicationNode::UpdateConfigFile(const Json::Value& config)
{
    YAML::Node yaml_node;

    ubt_yaml::JsonToYaml(config, yaml_node);

    std::ofstream fout(ConfigDataInfo::Instance().getConfigFilePath().c_str());
    fout << yaml_node;
    fout.close();
}

void CUbtCommunicationNode::RequestConfigMessage(const Json::Value& root)
{
    YAML::Node yaml_node;
    Json::Value config;

    try
    {
        yaml_node = YAML::LoadFile(ConfigDataInfo::Instance().getConfigFilePath());
    }
    catch (const std::exception& e)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"yaml load error: " << e.what() << '\n');
        return;
    }

    for (auto it = yaml_node.begin(); it != yaml_node.end(); ++it)
    {
        config[it->first.as<std::string>()] = it->second.as<std::string>();
    }

    std::string requsetid = root["request_id"].asString();
    std::string res_topic = root["data"]["res_topic"].asString();
    std::string data = _pMessageStr->PackRequsetConfigMessage(requsetid.replace(0, 5, "res_"), config); // strlen("send_") = 5

    RCLCPP_INFO_STREAM(node->get_logger(),"data: " << data);
    _pMqttStr->Publish(res_topic.c_str(), data.length(), data.c_str());
}

bool CUbtCommunicationNode::StartMapping()
{
    std::string type = ConfigDataInfo::Instance().getRobotAlgorithmType();

    if (_MappingStatus)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"can not start mapping , mapping status: " << _MappingStatus);
        return false;
    }

    if (type == ROBOT_ALGORITHN_TYPE_AMR)
    {
        _pMapStr->StartGmappingLaunch();
    }
    else if (type == ROBOT_ALGORITHN_TYPE_USLAM)
    {
        std_msgs::msg::Int32 msg;

        msg.data = 5100;
        _pubCtrlGmapping->publish(msg);
    }

    _MappingStatus = true;

    std_msgs::msg::String msg;
    msg.data = "start_carto";
    _pubCtrlCarto->publish(msg);
    return true;
}

bool CUbtCommunicationNode::StopMapping()
{
    if (!_MappingStatus)
    {
        RCLCPP_ERROR_STREAM(node->get_logger(),"can not stop mapping , mapping status: " << _MappingStatus);
        return false;
    }

    if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_AMR)
    {
        std_msgs::msg::String msg;
        msg.data = "stop_carto";
        _pubCtrlCarto->publish(msg);
    }
    else if (ConfigDataInfo::Instance().getRobotAlgorithmType() == ROBOT_ALGORITHN_TYPE_USLAM)
    {
        std_msgs::msg::Int32 msg;

        msg.data = 5102;
        _pubCtrlGmapping->publish(msg);
        _UslamMapFinished = false;
    }

    _MappingStatus = false;
    return true;
}

void CUbtCommunicationNode::LoopThread(void)
{
    while (rclcpp::ok())
    {
        if (_PublishMamager[E_HEART_BEAT_MANAGER].need_send(g_publish_message_rate[E_HEART_BEAT_MANAGER]))
        {
            std::string data = _pMessageStr->PackHeartBeatMessage();
            //RCLCPP_INFO_STREAM(node->get_logger(),"heartbeat - data: " << data);
            _pMqttStr->Publish("agv/command/heartbeat/", data.length(), data.c_str());
        }

        if (_PublishMamager[E_MAP_TASK_INFO_MESSAGE].need_send(g_publish_message_rate[E_MAP_TASK_INFO_MESSAGE]))
        {
            // RCLCPP_INFO_STREAM(node->get_logger(),"data: " << _TaskPublishData);
            _pMqttStr->Publish("agv/publicMsg/task/", _TaskPublishData.length(), _TaskPublishData.c_str());
        }

        if (_PublishMamager[E_SEND_AGV_VERSION].need_send(g_publish_message_rate[E_SEND_AGV_VERSION]))
        {
            std::string data = _pMessageStr->PackVersionInfoMessage(_RobotVersion);
            // RCLCPP_INFO_STREAM(node->get_logger(),"data: " << data);
            _pMqttStr->Publish("agv/publicMsg/version", data.length(), data.c_str());
        }

        // check mqtt connect status
        if (_PublishMamager[E_CHECK_MQTT_CONNECT_STATUS].need_send(g_publish_message_rate[E_CHECK_MQTT_CONNECT_STATUS]))
        {
            std_msgs::msg::UInt8 msg;

            msg.data = (_pMqttStr->MqttConnectStatus() == true) ? 1 : 2;
            _pubMqttStatus->publish(msg);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
}

}


